#pragma once

#include <Math/Matrix3x3.hpp>
#include "Quaternion.hpp"
#include "Transformation.hpp"

namespace zzz{
template <typename T>
class Rotation : public Matrix<3,3,T>
{
public:
  Rotation():Matrix<3,3,T>(1,0,0,0,1,0,0,0,1){}
  Rotation(const Rotation<T> &other):Matrix<3,3,T>(other){}
  explicit Rotation(const MatrixBase<3,3,T> &other):Matrix<3,3,T>(other){}
  explicit Rotation(const Quaternion<T> &q)
  {
    T* v=Data();
    v[0] = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]);
    v[1] = 2.0 * (q[0] * q[1] - q[2] * q[3]);
    v[2] = 2.0 * (q[0] * q[2] + q[1] * q[3]);          

    v[3] = 2.0 * (q[0] * q[1] + q[2] * q[3]);
    v[4]= 1.0 - 2.0 * (q[0] * q[0] + q[2] * q[2]);
    v[5] = 2.0 * (q[1] * q[2] - q[0] * q[3]);

    v[6] = 2.0 * (q[0] * q[2] - q[1] * q[3]);
    v[7] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
    v[8] = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]);
  }
  Rotation(const Vector<3,T> &normal, const T angle)
  {
    if (normal.Len()==0) Diagonal(1);
    else *this=Rotation<T>(Quaternion<T>(normal,angle));
  }

  // pitch -> x-axis
  // yaw -> y-axis
  // roll -> z-axis
  Rotation(T pitch, T yaw, T roll)
  {
    T *v=Data();
    v[0]=cos(pitch)*cos(yaw);
    v[1]=-cos(roll)*sin(yaw)+sin(roll)*sin(pitch)*cos(yaw);
    v[2]=sin(roll)*sin(yaw)+cos(roll)+sin(pitch)*cos(yaw);
    v[3]=cos(pitch)*sin(yaw);
    v[4]=cos(pitch)*cos(yaw)+sin(roll)*sin(pitch)*sin(yaw);
    v[5]=-sin(roll)*cos(yaw)+cos(roll)*sin(pitch)*sin(yaw);
    v[6]=-sin(pitch);
    v[7]=sin(roll)*cos(pitch);
    v[8]=cos(roll)*cos(pitch);
  }
  
  void ToEulerAngles(T &pitch, T &yaw, T &roll)
  {
    T *v=Data();
    if (v[6] == -1) {
      roll = 0;
      yaw = C_PI_2;
      pitch = atan2(v[1], v[2]);
    } else if (v[6] == 1) {
      roll = 0;
      yaw = -C_PI_2;
      pitch = atan2(-v[1], -v[2]);
    } else {
      yaw = -asin(v[6]);
      pitch = atan2(v[7]/cos(yaw), v[8]/cos(yaw));
      roll = atan2(v[9]/cos(yaw), v[0]/cos(yaw));
    }
  }

  inline static Rotation<T> GetIdentical() {
    Rotation<T> mat;
    mat.Identical();
    return mat;
  }

  using Matrix<3,3,T>::operator=;
  using Matrix<3,3,T>::Data;
  using Matrix<3,3,T>::Diagonal;
  
  VectorBase<3,T> Apply(const VectorBase<3,T> &a) const
  {
    return (*this)*a;
  }

  //OpenGL
  void ApplyGL() const
  {
    GLTransformation<T>(*this).ApplyGL();
  }
};

typedef Rotation<zfloat32> Rotationf32;
typedef Rotation<zfloat64> Rotationf64;

typedef Rotation<float> Rotationf;
typedef Rotation<double> Rotationd;

}
